#include "vehicle_dyn_factory.hpp"
#include "test_vehicle_dyn_path.h"
#include "common_tool/csv_writer.h"
using namespace auto_ros::control;
int main()
{
	std::string log_file_name = test_vehicle_dyn_path + "tire_angle.csv";
	std::shared_ptr<io::CSVWriter> log_writer_ptr_ = std::make_shared<io::CSVWriter>(log_file_name);
	std::vector<std::string> header = {"speed", "f_angle", "r_angle"};
	log_writer_ptr_->write_row<std::string>(header);
	std::string dyn_yaml_path_name = test_vehicle_dyn_path + "vehicle_linear_4d4s/"
															 "vehicle_linear_4d4s.yaml";
	VehicleDynFactory test_dyn_factory;
	std::shared_ptr<VehicleDynBase> test_dyn =
		test_dyn_factory.CreateSharedObject("VehicleLinear4d4s", dyn_yaml_path_name);
	std::cout << "speed =1.0,state_matrix is:" << test_dyn->state_matrix() << std::endl;
	for (double speed = 4; speed < 24; speed += 0.5)
	{

		test_dyn->para_update_matrix(speed);
		std::cout << "speed =" << speed << ",state_matrix is:" << test_dyn->state_matrix() << std::endl;
		std::cout << test_dyn->dyn_yaml_ << std::endl;

		//	bool calc_steady_state_ctr(
		//	const Eigen::VectorXd &distur_ref, const Eigen::VectorXd &y_controlled_ref,
		//	Eigen::VectorXd &steady_state, Eigen::VectorXd &steady_control);
		Eigen::VectorXd distur_ref(test_dyn->distur_dim());
		distur_ref << 0.02, 2.0, 0.0, 0.0, 0.0;
		Eigen::VectorXd y_controlled_ref(test_dyn->out_dim());
		y_controlled_ref << 0.0, 0.0, 0.0;
		Eigen::VectorXd steady_state(test_dyn->state_dim());
		Eigen::VectorXd steady_control(test_dyn->control_dim());
		std::map<std::string, double> par_map = {{"ax", 2.0}, {"ay", speed * speed * distur_ref(0)}};
		std::cout << test_dyn->state_dim() << std::endl;
		if (test_dyn->calc_steady_state_ctr(distur_ref, y_controlled_ref,
											steady_state, steady_control, par_map) == false)
		{
			std::cout << "\033[31m Error::calc_steady_state_ctr failed in test_dyn \033[0m" << std::endl;
		}
		else
		{

			std::cout << "steady _tate is:" << std::endl
					  << steady_state << std::endl;
			std::cout << "steady_contorl is:" << std::endl
					  << steady_control << std::endl;
			std::vector<double> tire_angle_vec = {speed, steady_control(0), steady_control(1)};
			log_writer_ptr_->write_row<double>(tire_angle_vec);
		}
	}
}